#include "focHal.h"
#include "main.h"

static foc_p foc = NULL;

extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim2;

void pwmInit(void)
{
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
}

float ua, ub, uc;
void setPwm(float Ua, float Ub, float Uc)
{
    ua = Ua;
    ub = Ub;
    uc = Uc;
    // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, Ua * PWM_PERIOD);
    // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, Ub * PWM_PERIOD);
    // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, Uc * PWM_PERIOD);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, Ua);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, Ub);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, Uc);
}

void focHalInit(void)
{
    foc = newFoc(0, 7, 24, setPwm);
}

void focHalLoop(void)
{
    foc->loop(foc, HAL_GetTick());
}